/*
  File:    Curvature.cpp
  Purpose: Advanced routines for level set-based curvature and normal vector 
           calculations. Fully adaptive for level set irregularities. 

  Contact: Paul Macklin
           pmacklin@math.uci.edu
		  http://math.uci.edu/~pmacklin
*/

// #include "Curvature.h"
// #include "GeometricData.h"

#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <cmath>

using namespace std;

#include "../ImprovedMath.h"
#include "../Matrix.h"
#include "../random.h"

#include "Contour.h"
#include "Polynomial.h"
#include "LeastSquares.h"

#include "GeometricData.h"
  
Matrix GlobalCurvature;

Matrix GlobalQualityMatrix;
Matrix GlobalGeometryDecisionMatrix;

Matrix GlobalNormalX;
Matrix GlobalNormalY;

double GeometryTolerance;

Contour GlobalZeroContour;

GeometricDataPacket LevelSetGeometricData( Matrix &Xin, Matrix &Yin, Matrix &PhiIn, int i, int j )
{
 extern double _EPS;
 extern double _PI;

 GeometricDataPacket Output;
 Output.Curvature = 0;
 Output.NormalX   = 0;
 Output.NormalY   = 0;

 double DeltaX = *Xin(1) - *Xin(0);
 double DeltaY = *Yin(1) - *Yin(0);
 double px = (*PhiIn(i+1,j)-*PhiIn(i-1,j))/(2.0*DeltaX);
 double px2 = square(px);
 double py = (*PhiIn(i,j+1)-*PhiIn(i,j-1))/(2.0*DeltaY);
 double py2 = square(py);
 double pxx = (*PhiIn(i-1,j) - 2.0*(*PhiIn(i,j)) + *PhiIn(i+1,j))/square(DeltaX);
 double pyy = (*PhiIn(i,j-1) - 2.0*(*PhiIn(i,j)) + *PhiIn(i,j+1))/square(DeltaY);
 double pxy = (*PhiIn(i+1,j+1)-*PhiIn(i-1,j+1)-*PhiIn(i+1,j-1)+*PhiIn(i-1,j-1))/(4.0*DeltaX*DeltaY);
 
 double norm = sqrt( px2 + py2 + _EPS );
 
 Output.NormalX   = px / norm;
 Output.NormalY   = py / norm;
 Output.Curvature = (pxx*py2 - 2.0*px*py*pxy + pyy*px2)/IntPow(norm,3);
 
 if( norm < 1e-7 )
 {
  double Angle = 2.0*_PI*UniformRandom();
  Output.NormalX = cos( Angle );
  Output.NormalY = sin( Angle );
 }
 
 return Output;
}

double DistanceToCurve(double x, double y, Polynomial& Xcurve, Polynomial& Ycurve )
{
 double S = 0.0;
 double DistanceSquared = square( x ) + square( y );
 double DeltaS = 0.1; 
 int Direction = 1;

 double PreviousDistanceSquared = 2.0 * DistanceSquared; 
 
 double CurrentX = 0;
 double CurrentY = 0;

 while( DeltaS > 1e-8 )
 {
  PreviousDistanceSquared = DistanceSquared;
  S += Direction*DeltaS;
  CurrentX = Xcurve(S);
  CurrentY = Ycurve(S);
  DistanceSquared = square( x - CurrentX ) + square( y - CurrentY );
  if( PreviousDistanceSquared < DistanceSquared )
  {
   Direction *= -1;
   PreviousDistanceSquared = 2*DistanceSquared;
   DeltaS *= 0.5; 
  }
 }
 return sqrt( DistanceSquared );
}

double ClosestS(double x, double y, Polynomial& Xcurve, Polynomial& Ycurve )
{
 double S = 0.0;
 double DistanceSquared = square( x ) + square( y );
 double DeltaS = 0.1; 
 int Direction = 1;

 double PreviousDistanceSquared = 2.0 * DistanceSquared; 
 
 double CurrentX = 0;
 double CurrentY = 0;

 while( DeltaS > 1e-10 )
 {
  PreviousDistanceSquared = DistanceSquared;
  S += Direction*DeltaS;
  CurrentX = Xcurve(S);
  CurrentY = Ycurve(S);
  DistanceSquared = square( x - CurrentX ) + square( y - CurrentY );
  if( PreviousDistanceSquared < DistanceSquared )
  {
   Direction *= -1;
   PreviousDistanceSquared = 2*DistanceSquared;
   DeltaS *= 0.5; 
  }
 }
 return S - Direction*DeltaS;
}

Matrix GenerateQualityMatrix( Matrix& Xin, Matrix& Yin, Matrix &PhiIn )
{
 Matrix Output = zeros( PhiIn.TellRows() , PhiIn.TellCols() );
 double dx_local = *Xin(1) - *Xin(0);
 double dy_local = *Yin(1) - *Yin(0);
 
 for( int j=1 ; j < PhiIn.TellCols()-1 ; j++ )
 {
  for( int i=1 ; i < PhiIn.TellRows()-1 ; i++ )
  {
   double nx = (*PhiIn(i+1,j)-*PhiIn(i-1,j))/(2.0*dx_local); 
   double ny = (*PhiIn(i,j+1)-*PhiIn(i,j-1))/(2.0*dy_local); 
   *Output(i,j) = fabs( 1.0 - sqrt( square(nx)+square(ny) ) );
  }
 }
 
 return Output;
}

bool GenerateGeometricGlobals( Matrix &Xin, Matrix &Yin, Matrix& PhiIn )
{
 int i,j;
 
 double dx_local = *Xin(1) - *Xin(0);
 double dy_local = *Yin(1) - *Yin(0);
 GeometryTolerance = 0.1*dx_local; // 1*dx; // use a large value to disable it completely
 
 GlobalQualityMatrix = GenerateQualityMatrix( Xin,Yin,PhiIn );
 GlobalGeometryDecisionMatrix = ones( Xin.TellCols() , Yin.TellCols() );

 for( i=1 ; i < Xin.TellCols()-1 ; i++ )
 {
  for( j=1 ; j < Yin.TellCols()-1 ; j++ )
  {
   
   for( int ii=i-1 ; ii <= i+1 ; ii++ )
   {
    for( int jj=j-1 ; jj <= j+1 ; jj++ )
	{
     if( *GlobalQualityMatrix(i,j) > GeometryTolerance )
	 {
      *GlobalGeometryDecisionMatrix(i,j) = 0.0; 
     }
	}
   }
  
  }
 }
 
 GlobalCurvature = zeros( Xin.TellCols() , Yin.TellCols() );
 GlobalNormalX = GlobalCurvature;
 GlobalNormalY = GlobalNormalX;
 
 for( i=1 ; i < Xin.TellCols()-1 ; i++ )
 {
  for( j=1 ; j < Yin.TellCols()-1 ; j++ )
  {
   GeometricDataPacket GDP = LevelSetGeometricData( Xin, Yin, PhiIn, i,j );
   *GlobalNormalX(i,j) = GDP.NormalX;
   *GlobalNormalY(i,j) = GDP.NormalY;
   *GlobalCurvature(i,j) = GDP.Curvature;
   if( *GlobalCurvature(i,j) > 1/dx_local )
   { *GlobalCurvature(i,j) = 1/dx_local; }
   if( *GlobalCurvature(i,j) < -1/dx_local )
   { *GlobalCurvature(i,j) = -1/dx_local; }
  }
 } 
 
 // clean out the old zero contour
 
 GlobalZeroContour.CleanupContour();
 
 // find the current zero contour
 
 GlobalZeroContour.InitializeFromMatrix( Xin,Yin,PhiIn, 0.0 );
 
 return true;
}

GeometricDataPacket AdaptiveGeometricData( Matrix& Xin, Matrix& Yin, Matrix& PhiIn, double x, double y )
{
 extern double _EPS;
 extern Matrix X;
 extern Matrix Y;

 GeometricDataPacket GDPout;
 
 bool Xdir = false;
 bool Ydir = false;
 
 double dx_local = *Xin(1) - *Xin(0);
 double dy_local = *Yin(1) - *Yin(0);

 int i=Xin.TellCols()-1;
 while( *Xin(i) > x && i > 0 ){ i--; } 
 if( fabs( *Xin(i) - x ) < _EPS ){ x = *Xin(i); Ydir = true; }

 int j=Yin.TellCols()-1;
 while( *Yin(j) > y && j > 0 ){ j--; } 
 if( fabs( *Yin(j) - y ) < _EPS ){ y = *Yin(j); Xdir = true; }

/* 
 // temporary!
 
 GDPout.Curvature = *GlobalCurvature(i,j);
 GDPout.NormalX = *GlobalNormalX(i,j);
 GDPout.NormalY = *GlobalNormalY(i,j);
 return GDPout;
*/ 

 Xdir = false;
 Ydir = false;
 
 if( Xdir && Ydir )
 {
  if( *GlobalGeometryDecisionMatrix(i,j) < 0.5 &&
      i > 3 && j > 3 &&
      i < X.TellCols()-3 && 
      j < Y.TellCols()-3 )
  {
   double phi = *PhiIn(i,j);
 //  Contour LC = FindLocalContour( Xin,Yin,PhiIn,x,y, phi );  
   GDPout = GlobalZeroContour.GetGeometricData(x,y); // ContourGeometricData( LC );
//   GDPout = AdvancedContourGeometricData( LC , 0.1*dx,0.1*dy ); 
  }
  else
  {
   GDPout.Curvature = *GlobalCurvature(i,j);
   GDPout.NormalX   = *GlobalNormalX(i,j);
   GDPout.NormalY   = *GlobalNormalY(i,j);
  }
 
  return GDPout;
 }

 if( Xdir )
 {
  double Theta = (x - *Xin(i))/dx_local;
    
  if( *GlobalGeometryDecisionMatrix(i-1,j) > 0.5 &&
      *GlobalGeometryDecisionMatrix(i,j)   > 0.5 && 
	  *GlobalGeometryDecisionMatrix(i+1,j) > 0.5 &&
	  *GlobalGeometryDecisionMatrix(i+2,j) > 0.5 )
  {
   double OneMinusTheta = 1.0-Theta;
   double OnePlusTheta  = 1.0+Theta;
   double TwoMinusTheta = 2.0-Theta;

   GDPout.Curvature = -(*GlobalCurvature(i-1,j))*Theta*OneMinusTheta*TwoMinusTheta/6.0
                      +(*GlobalCurvature(i,j))*OnePlusTheta*OneMinusTheta*TwoMinusTheta*0.5 
                      +(*GlobalCurvature(i+1,j))*OnePlusTheta*Theta*TwoMinusTheta*0.5
                      -(*GlobalCurvature(i+2,j))*OnePlusTheta*Theta*OneMinusTheta/6.0;
					  
   GDPout.NormalX   = -(*GlobalNormalX(i-1,j))*Theta*OneMinusTheta*TwoMinusTheta/6.0
                      +(*GlobalNormalX(i,j))*OnePlusTheta*OneMinusTheta*TwoMinusTheta*0.5 
                      +(*GlobalNormalX(i+1,j))*OnePlusTheta*Theta*TwoMinusTheta*0.5
                      -(*GlobalNormalX(i+2,j))*OnePlusTheta*Theta*OneMinusTheta/6.0;
					  
   GDPout.NormalY   = -(*GlobalNormalY(i-1,j))*Theta*OneMinusTheta*TwoMinusTheta/6.0
                      +(*GlobalNormalY(i,j))*OnePlusTheta*OneMinusTheta*TwoMinusTheta*0.5 
                      +(*GlobalNormalY(i+1,j))*OnePlusTheta*Theta*TwoMinusTheta*0.5
                      -(*GlobalNormalY(i+2,j))*OnePlusTheta*Theta*OneMinusTheta/6.0;

   return GDPout;
  }  
 
  if( *GlobalGeometryDecisionMatrix(i,j) > 0.5 &&
      *GlobalGeometryDecisionMatrix(i+1,j) > 0.5 )
  {
   double OneMinusTheta = 1.0-Theta;

   GDPout.Curvature = (*GlobalCurvature(i,j))*OneMinusTheta
                     +(*GlobalCurvature(i+1,j))*Theta;
					 
   GDPout.NormalX   = (*GlobalNormalX(i,j))*OneMinusTheta
                     +(*GlobalNormalX(i+1,j))*Theta;
					 
   GDPout.NormalY   = (*GlobalNormalY(i,j))*OneMinusTheta
                     +(*GlobalNormalY(i+1,j))*Theta;
   return GDPout;
  }
 
  double phi = (1.0-Theta)*(*PhiIn(i,j)) + Theta*(*PhiIn(i+1,j));
//  Contour LC = FindLocalContour( Xin,Yin,PhiIn,x,y, phi );
  GDPout = GlobalZeroContour.GetGeometricData(x,y); // ContourGeometricData( LC );
  
  return GDPout; 
 }

 if( Ydir )
 {
  double Theta = (y - *Yin(j))/dy_local; 
    
  if( *GlobalGeometryDecisionMatrix(i,j-1) > 0.5 &&
      *GlobalGeometryDecisionMatrix(i,j)   > 0.5 && 
	  *GlobalGeometryDecisionMatrix(i,j+1) > 0.5 &&
	  *GlobalGeometryDecisionMatrix(i,j+2) > 0.5 )
  {
   double OneMinusTheta = 1.0-Theta;
   double OnePlusTheta  = 1.0+Theta;
   double TwoMinusTheta = 2.0-Theta;

   GDPout.Curvature = -(*GlobalCurvature(i,j-1))*Theta*OneMinusTheta*TwoMinusTheta/6.0
                      +(*GlobalCurvature(i,j))*OnePlusTheta*OneMinusTheta*TwoMinusTheta*0.5 
                      +(*GlobalCurvature(i,j+1))*OnePlusTheta*Theta*TwoMinusTheta*0.5
                      -(*GlobalCurvature(i,j+2))*OnePlusTheta*Theta*OneMinusTheta/6.0;
					  
   GDPout.NormalX   = -(*GlobalNormalX(i,j-1))*Theta*OneMinusTheta*TwoMinusTheta/6.0
                      +(*GlobalNormalX(i,j))*OnePlusTheta*OneMinusTheta*TwoMinusTheta*0.5 
                      +(*GlobalNormalX(i,j+1))*OnePlusTheta*Theta*TwoMinusTheta*0.5
                      -(*GlobalNormalX(i,j+2))*OnePlusTheta*Theta*OneMinusTheta/6.0;
					  
   GDPout.NormalY   = -(*GlobalNormalY(i,j-1))*Theta*OneMinusTheta*TwoMinusTheta/6.0
                      +(*GlobalNormalY(i,j))*OnePlusTheta*OneMinusTheta*TwoMinusTheta*0.5 
                      +(*GlobalNormalY(i,j+1))*OnePlusTheta*Theta*TwoMinusTheta*0.5
                      -(*GlobalNormalY(i,j+2))*OnePlusTheta*Theta*OneMinusTheta/6.0;

   return GDPout;
  }  
 
  if( *GlobalGeometryDecisionMatrix(i,j) > 0.5 &&
      *GlobalGeometryDecisionMatrix(i,j+1) > 0.5 )
  {
   double OneMinusTheta = 1.0-Theta;

   GDPout.Curvature = (*GlobalCurvature(i,j))*OneMinusTheta
                     +(*GlobalCurvature(i,j+1))*Theta;
					 
   GDPout.NormalX   = (*GlobalNormalX(i,j))*OneMinusTheta
                     +(*GlobalNormalX(i,j+1))*Theta;
					 
   GDPout.NormalY   = (*GlobalNormalY(i,j))*OneMinusTheta
                     +(*GlobalNormalY(i,j+1))*Theta;

   return GDPout;
  }
  double phi = (1.0-Theta)*(*PhiIn(i,j)) + Theta*(*PhiIn(i,j+1));

//  Contour LC = FindLocalContour( Xin,Yin,PhiIn,x,y, phi );
  GDPout = GlobalZeroContour.GetGeometricData(x,y);  // ContourGeometricData( LC );
  return GDPout; 
 }
 
 double ThetaX = (x-*Xin(i))/dx_local;
 double ThetaY = (y-*Yin(j))/dy_local;
 double OneMinusThetaX = 1.0-ThetaX;
 double OneMinusThetaY = 1.0-ThetaY;

 if( *GlobalGeometryDecisionMatrix(i,j)     > 0.5 &&
     *GlobalGeometryDecisionMatrix(i+1,j)   > 0.5 &&
	 *GlobalGeometryDecisionMatrix(i,j+1)   > 0.5 &&
     *GlobalGeometryDecisionMatrix(i+1,j+1) > 0.5 )
 {
  GDPout.Curvature = OneMinusThetaY*( (*GlobalCurvature(i,j))    *OneMinusThetaX
                                     +(*GlobalCurvature(i+1,j))  *ThetaX )
			            + ThetaY*( (*GlobalCurvature(i,j+1))  *OneMinusThetaX 
				                 +(*GlobalCurvature(i+1,j+1))*ThetaX );
								 
  GDPout.NormalX   = OneMinusThetaY*( (*GlobalNormalX(i,j))    *OneMinusThetaX
                                     +(*GlobalNormalX(i+1,j))  *ThetaX )
			            + ThetaY*( (*GlobalNormalX(i,j+1))  *OneMinusThetaX 
				                 +(*GlobalNormalX(i+1,j+1))*ThetaX );
								 
  GDPout.NormalY   = OneMinusThetaY*( (*GlobalNormalY(i,j))    *OneMinusThetaX
                                     +(*GlobalNormalY(i+1,j))  *ThetaX )
			            + ThetaY*( (*GlobalNormalY(i,j+1))  *OneMinusThetaX 
				                 +(*GlobalNormalY(i+1,j+1))*ThetaX );
								 
  return GDPout;
 }

 double phi = OneMinusThetaY*( (*PhiIn(i,j))    *OneMinusThetaX 
                              +(*PhiIn(i+1,j))  *ThetaX )
                    + ThetaY*( (*PhiIn(i,j+1))  *OneMinusThetaX 
				          +(*PhiIn(i+1,j+1))*ThetaX );
// Contour LC = FindLocalContour( Xin,Yin,PhiIn,x,y, phi );
 GDPout = GlobalZeroContour.GetGeometricData(x,y);  // ContourGeometricData( LC );
 return GDPout;
}

bool OutputGeometricData( char* NormalXname, char* NormalYname, char* CurvatureName )
{
 extern Matrix X;
 extern Matrix Y;
 extern Matrix Phi;
 extern int Xnodes;
 extern int Ynodes;

 int i,j;
 
 Matrix TempCurvature = zeros( Xnodes, Ynodes );
 Matrix TempNormalX   = zeros( Xnodes, Ynodes );
 Matrix TempNormalY   = zeros( Xnodes, Ynodes );
 
 for( j=1 ; j < Ynodes-1 ; j++ )
 {
  for( i=1 ; i < Xnodes-1 ; i++ )
  {
   if( *Phi(i,j) < 1 )
   {
    GeometricDataPacket GDP = AdaptiveGeometricData( X,Y,Phi,*X(i),*Y(j) );
    *TempNormalX(i,j) = GDP.NormalX;
    *TempNormalY(i,j) = GDP.NormalY;
    *TempCurvature(i,j)= GDP.Curvature;
   }
  }
 }
 
 TempCurvature.WriteToFile( CurvatureName );
 TempNormalX.WriteToFile(   NormalXname   );
 TempNormalY.WriteToFile(   NormalYname   );

 return true;
}
